using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.Core.DsspHttp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using Microsoft.Robotics.PhysicalModel.Proxy;
using System;
using System.Net;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.ComponentModel;
using System.Security.Permissions;
using xml = System.Xml;
using W3C.Soap;

using bumper = Microsoft.Robotics.Services.ContactSensor.Proxy;
using vex = VEXRobotics.Proxy;
using submgr = Microsoft.Dss.Services.SubscriptionManager;

/* Notes on VEX Binary Sensor
 *   Identifier must be the port number on the VEX. Range: 1 - 16.
 */

namespace VEXService.ContactSensor
{
    /// <summary>
    /// ContactSensorArray Port
    /// </summary>
    [ServicePort]
    public class ContactSensorArrayOperations : PortSet<
        DsspDefaultLookup,
        DsspDefaultDrop,
        Get,
        bumper.Replace,
        bumper.Update,
        bumper.ReliableSubscribe,
        bumper.Subscribe,
        HttpGet>
    {
    }


    /// <summary>
    /// Get the VEX Bumper Sensor State
    /// </summary>
    [Description("Gets the current state of the VEX Contact Sensor.")]
    public class Get : Get<GetRequestType, PortSet<VEXBumperState, Fault>>
    {
    }

    /// <summary>
    /// Additional configuration which is necessary to 
    /// configure any VEX sensor as a contact sensor.
    /// </summary>
    [DataContract]
    public class VEXBumperConfig : IDssSerializable, ICloneable
    {
        private vex.SensorPort _sensorPort;
        private vex.SensorDefinition.SensorType _sensorType;
        private string _name;
        private Pose _pose;
        private bool _pressed;
        private DateTime _timestamp;
        private int _thresholdLow;
        private int _thresholdHigh;

        public VEXBumperConfig()
        {
            _pose = new Pose();
        }

        /// <summary>
        /// Indentifies the VEX Port (1-16) in which this sensor is plugged.
        /// </summary>
        [DataMember]
        [Description("The port (1-16) in which the sensor is plugged.")]
        public vex.SensorPort SensorPort
        {
            get { return _sensorPort; }
            set { _sensorPort = value; }
        }

        /// <summary>
        /// The name of this sensor
        /// </summary>
        [DataMember]
        [Description("The descriptive name for this sensor.")]
        public string Name
        {
            get { return _name; }
            set { _name = value; }
        }

        /// <summary>
        /// One of the VEXConfig.AvailableSensorTypes
        /// </summary>
        [DataMember]
        [Description("The sensor types.")]
        public vex.SensorDefinition.SensorType SensorType
        {
            get { return _sensorType; }
            set { _sensorType = value; }
        }

        /// <summary>
        /// The lowest value which will trigger a bumper event
        /// </summary>
        [DataMember]
        [Description("The lowest value which will trigger a contact event.")]
        public int ThresholdLow
        {
            get { return _thresholdLow; }
            set { _thresholdLow = value; }
        }

        /// <summary>
        /// The highest value which will trigger a bumper event
        /// </summary>
        [DataMember]
        [Description("The highest value which will trigger a contact event.")]
        public int ThresholdHigh
        {
            get { return _thresholdHigh; }
            set { _thresholdHigh = value; }
        }


        /// <summary>
        /// Pose of the sensor
        /// </summary>
        [DataMember]
        [Description("The position and orientation of the light sensor.")]
        public Pose Pose
        {
            get { return _pose; }
            set { _pose = value; }
        }

        /// <summary>
        ///     Whether the button is currently pressed
        /// </summary>
        [Browsable(false)]
        [Description("Whether the button has been pressed")]
        [DataMember(Order = -1)]
        public bool Pressed
        {
            get { return _pressed; }
            set { _pressed = value; }
        }

        /// <summary>
        /// The time of the last sensor update
        /// </summary>
        [DefaultValue(typeof(DateTime), "0001-01-01T00:00:00")]
        [Description("Identifies the timestamp of the sensor reading.")]
        [Browsable(false)]
        [DataMember(Order = -1, XmlOmitDefaultValue = true)]
        public DateTime TimeStamp
        {
            get { return _timestamp; }
            set { _timestamp = value; }
        }


        #region IDssSerializable

        /// <summary>
        /// Copy To light sensor state
        /// </summary>
        public virtual void CopyTo(IDssSerializable target)
        {
            VEXBumperConfig typedTarget = target as VEXBumperConfig;

            if (typedTarget == null)
                throw new ArgumentException("CopyTo({0}) requires type {0}", this.GetType().FullName);

            typedTarget.SensorPort = this.SensorPort;
            typedTarget.Name = this.Name;
            typedTarget.Pose = this.Pose;
            typedTarget.Pressed = this.Pressed;
            typedTarget.SensorType = this.SensorType;
            typedTarget.ThresholdHigh = this.ThresholdHigh;
            typedTarget.ThresholdLow = this.ThresholdLow;
            typedTarget.TimeStamp = this.TimeStamp;
        }

        /// <summary>
        /// Clone light sensor state
        /// </summary>
        public virtual object Clone()
        {
            VEXBumperConfig target = new VEXBumperConfig();

            target.SensorPort = this.SensorPort;
            target.Name = this.Name;
            target.Pose = this.Pose;
            target.Pressed = this.Pressed;
            target.SensorType = this.SensorType;
            target.ThresholdHigh = this.ThresholdHigh;
            target.ThresholdLow = this.ThresholdLow;
            target.TimeStamp = this.TimeStamp; 
            return target;

        }
        /// <summary>
        /// Serialize
        /// </summary>
        public virtual void Serialize(System.IO.BinaryWriter writer)
        {
            writer.Write(Name);
            Pose.Serialize(writer);
            writer.Write(Pressed);
            writer.Write((int)SensorPort);
            writer.Write((int)SensorType);
            writer.Write(ThresholdHigh);
            writer.Write(ThresholdLow);
            Microsoft.Dss.Services.Serializer.BinarySerializationHelper.SerializeDateTime(TimeStamp, writer);
        }

        /// <summary>
        /// Deserialize
        /// </summary>
        public virtual object Deserialize(System.IO.BinaryReader reader)
        {
            Name = reader.ReadString();
            Pose.Deserialize(reader);
            Pressed = reader.ReadBoolean();
            SensorPort = (vex.SensorPort)reader.ReadInt32();
            SensorType = (vex.SensorDefinition.SensorType)reader.ReadInt32();
            ThresholdHigh = reader.ReadInt32();
            ThresholdLow = reader.ReadInt32();
            TimeStamp = Microsoft.Dss.Services.Serializer.BinarySerializationHelper.DeserializeDateTime(reader);

            return this;
        }

        #endregion
    }

    /// <summary>
    /// A list of bumper configurations
    /// </summary>
    [DataContract]
    [Description("The sensor state.")]
    public class VEXBumperState : IDssSerializable, ICloneable
    {
        public VEXBumperState()
        {
            Sensors = new List<VEXBumperConfig>();
        }

        [DataMember(Order = -1)]
        [Description("The sensor configuration list.")]
        public List<VEXBumperConfig> Sensors;

        /// <summary>
        /// Transform current state to the specified generic state.
        /// </summary>
        /// <param name="genericState"></param>
        /// <returns></returns>
        public bumper.ContactSensorArrayState ToGenericState(bumper.ContactSensorArrayState genericState)
        {
            genericState.Sensors.Clear();
            for(int sensorIndex=0; sensorIndex<Sensors.Count; ++sensorIndex)
            {
                bumper.ContactSensor genericSensor = new bumper.ContactSensor();
                genericState.Sensors.Add(ToGenericState(genericSensor, sensorIndex));
            }
            return genericState;
        }

        /// <summary>
        /// Transform current state to the specified generic state for a specific sensor.
        /// </summary>
        /// <param name="genericState"></param>
        /// <returns></returns>
        public bumper.ContactSensor ToGenericState(bumper.ContactSensor genericSensor, int sensorIndex)
        {
            if (sensorIndex < 0 || sensorIndex >= Sensors.Count)
                return null;

            genericSensor.HardwareIdentifier = (int)Sensors[sensorIndex].SensorPort;
            genericSensor.Name = Sensors[sensorIndex].Name;
            genericSensor.Pose = Sensors[sensorIndex].Pose;
            genericSensor.Pressed = Sensors[sensorIndex].Pressed;
            genericSensor.TimeStamp = Sensors[sensorIndex].TimeStamp;

            return genericSensor;
        }

        /// <summary>
        /// Copy generic state to this state, transforming data members.
        /// </summary>
        /// <param name="genericState"></param>
        public void FromGenericState(bumper.ContactSensorArrayState genericState)
        {
            Sensors.Clear();
            foreach (bumper.ContactSensor genericSensor in genericState.Sensors)
            {
                VEXBumperConfig newSensor = new VEXBumperConfig();
                newSensor.Name = genericSensor.Name;
                newSensor.Pose = genericSensor.Pose;
                newSensor.Pressed = genericSensor.Pressed;
                if (genericSensor.HardwareIdentifier >= 0 && genericSensor.HardwareIdentifier <= 16)
                    newSensor.SensorPort = (vex.SensorPort)genericSensor.HardwareIdentifier;
                else
                    newSensor.SensorPort = vex.SensorPort.SensorPort_None;
                newSensor.SensorType = vex.SensorDefinition.SensorType.DigitalInput;
                newSensor.ThresholdHigh = 1;
                newSensor.ThresholdLow = 0;
                newSensor.TimeStamp = genericSensor.TimeStamp;

                Sensors.Add(newSensor);
            }
        }

        #region IDssSerializable

        /// <summary>
        /// Copy To sensor state
        /// </summary>
        public virtual void CopyTo(IDssSerializable target)
        {
            VEXBumperState typedTarget = target as VEXBumperState;

            if (typedTarget == null)
                throw new ArgumentException("CopyTo({0}) requires type {0}", this.GetType().FullName);

            typedTarget.Sensors.Clear();
            foreach(VEXBumperConfig sensor in Sensors)
            {
                typedTarget.Sensors.Add(sensor);
            }
        }

        /// <summary>
        /// Clone sensor state
        /// </summary>
        public virtual object Clone()
        {
            VEXBumperState target = new VEXBumperState();

            foreach (VEXBumperConfig sensor in Sensors)
            {
                target.Sensors.Add((VEXBumperConfig)sensor.Clone());
            }
            return target;

        }
        /// <summary>
        /// Serialize
        /// </summary>
        public virtual void Serialize(System.IO.BinaryWriter writer)
        {
            writer.Write(Sensors.Count);
            foreach (VEXBumperConfig sensor in Sensors)
            {
                sensor.Serialize(writer);
            }
        }

        /// <summary>
        /// Deserialize
        /// </summary>
        public virtual object Deserialize(System.IO.BinaryReader reader)
        {
            Sensors.Clear();

            int cnt = reader.ReadInt32();
            for (int i = 0; i < cnt; ++i)
            {
                VEXBumperConfig newSensor = new VEXBumperConfig();
                newSensor.Deserialize(reader);
                Sensors.Add(newSensor);
            }

            return this;
        }

        #endregion
    }

    /// <summary>
    /// VEX Bumper Contract 
    /// </summary>
    public sealed class Contract
    {
        public const string Identifier = "http://schemas.vexrobotics.com/2011/09/vexservice.contactsensor.html";
    }


    [Contract(Contract.Identifier)]
    [AlternateContract(bumper.Contract.Identifier)]
    [DisplayName("VEX Contact Sensors")]
    [Description("Provides access to the VEX sensors when used as a bumper or contact sensors.")]
    [DssServiceDescription("http://msdn.microsoft.com/en-us/library/dd145249.aspx")]
    [DssCategory(vex.VEXCategories.VEX)]
    public class VEXContactSensorService : DsspServiceBase
    {
        /// <summary>
        /// XSLT Transform to use for display
        /// </summary>
        [EmbeddedResource("VEXService.Config.ContactSensor.xslt")]
        private string _transform = null;

        /// <summary>
        /// Sensor array state
        /// </summary>
        [InitialStatePartner(Optional = true, ServiceUri = ServicePaths.Store + @"/VEX.Bumper.config.xml")]
        private VEXBumperState _State = new VEXBumperState();

        /// <summary>
        /// Service port
        /// </summary>
        [ServicePort("/VEX/ContactSensor", AllowMultipleInstances = true)]
        private ContactSensorArrayOperations _mainPort = new ContactSensorArrayOperations();

        /// <summary>
        /// Alternate port for the generic port
        /// </summary>
        [AlternateServicePort("/bumper",
            AllowMultipleInstances = true,
            AlternateContract = bumper.Contract.Identifier)]
        private bumper.ContactSensorArrayOperations _genericPort = new bumper.ContactSensorArrayOperations();

        /// <summary>
        /// Port to talk with the VEX controller
        /// </summary>
        [Partner("VEX", Contract = vex.Contract.Identifier,
            CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate,
            Optional = false)]
        private vex.VEXOperations _vexPort = new vex.VEXOperations();

        // VEX Notifications
        vex.VEXOperations _notificationPort = new vex.VEXOperations();

        /// <summary>
        /// Subscription manager
        /// </summary>
        [Partner("SubMgr", Contract = submgr.Contract.Identifier,
            CreationPolicy = PartnerCreationPolicy.CreateAlways,
            Optional = false)]
        private submgr.SubscriptionManagerPort _subMgrPort = new submgr.SubscriptionManagerPort();

        /// <summary>
        /// Constructor
        /// </summary>
        /// <param name="creationPort"></param>
        public VEXContactSensorService(DsspServiceCreationPort creationPort)
            : base(creationPort)
        {

        }

        #region Initialization Code

        /// <summary>
        /// Startup
        /// </summary>
        protected override void Start()
        {
            LogVerbose(LogGroups.Console, "Configuring VEX Contact Sensor Array... ");

            // Listen for each of my handlers
            ActivateDsspOperationHandlers();

            // Publish the service to the local Node Directory
            DirectoryInsert();

            #region Initialize Service State
            Port<bool> donePort = new Port<bool>();
            if (_State == null)
            {
                _State = new VEXBumperState();
                SpawnIterator<Port<bool>>(donePort, InitializeBumperConnection);
            }
            else
            {
                SpawnIterator<Port<bool>>(donePort, SetBumperConnection);
            }

            Activate(Arbiter.Receive(false, donePort,
                delegate(bool success)
                {
                    if (success)
                    {
                        // display HTTP service Uri
                        LogInfo(LogGroups.Console, "Service uri: ");
                    }
                    else
                    {
                        LogError(LogGroups.Console, "VEX Contact Sensors service failed to start.");
                        _mainPort.Post(new DsspDefaultDrop());
                    }

                }));
            #endregion
        }

        /// <summary>
        /// Set the configuration
        /// </summary>
        /// <param name="donePort"></param>
        /// <returns></returns>
        private IEnumerator<ITask> SetBumperConnection(Port<bool> donePort)
        {
            bool serviceSuccess = true;

            // Configure default state
            for (int i = 0; i < _State.Sensors.Count; ++i)
            {
                vex.SelectivePortConfigure newConfig = new vex.SelectivePortConfigure();
                newConfig.Body.Port = _State.Sensors[i].SensorPort;
                newConfig.Body.Type = _State.Sensors[i].SensorType;

                _vexPort.Post(newConfig);
                yield return Arbiter.Choice(newConfig.ResponsePort,
                    delegate(DefaultReplaceResponseType response)
                    {
                    },
                    delegate(Fault fault)
                    {
                        serviceSuccess = false;
                        LogError(LogGroups.Console, "Failed to update VEX service configuration", fault);
                    });
            }

            // Subscribe to VEX for notifications
            if (serviceSuccess)
            {
                serviceSuccess = false;
                SaveState(_State);
                Port<bool> subscribeDonePort = new Port<bool>();
                SpawnIterator<Port<bool>>(subscribeDonePort, SubscribeToVEX);
                yield return (Arbiter.Receive<bool>(false, subscribeDonePort,
                    delegate(bool success)
                    {
                        serviceSuccess = success;
                    }));
            }

            donePort.Post(serviceSuccess);
            yield break;
        }

        /// <summary>
        /// Multi-stage bumper initialization code
        /// </summary>
        /// <returns></returns>
        private IEnumerator<ITask> InitializeBumperConnection(Port<bool> donePort)
        {
            bool serviceSuccess = false;

            //configure default state
            Port<VEXBumperState> configBumperPort = new Port<VEXBumperState>();
            SpawnIterator<Port<VEXBumperState>>(configBumperPort, GetVEXBumperConfiguration);
            yield return (Arbiter.Receive<VEXBumperState>(false, configBumperPort,
                delegate(VEXBumperState newBumperState)
                {
                    _State = newBumperState;
                }));

            // Subscribe to VEX for notifications
            SaveState(_State);
            Port<bool> subscribeDonePort = new Port<bool>();
            SpawnIterator<Port<bool>>(subscribeDonePort, SubscribeToVEX);
            yield return (Arbiter.Receive<bool>(false, subscribeDonePort,
                delegate(bool success)
                {
                    serviceSuccess = success;
                }));

            donePort.Post(serviceSuccess);
            yield break;
        }

        /// <summary>
        /// Get state from VEX and set up a contact sensor
        /// for each of the configured ports.
        /// </summary>
        /// <param name="resultPort"></param>
        /// <returns></returns>
        private IEnumerator<ITask> GetVEXBumperConfiguration(Port<VEXBumperState> resultPort)
        {
            VEXBumperState configArray = new VEXBumperState();
            vex.VEXState vexState = null;
            bool existingBumperConfig = (_State != null && _State.Sensors != null && _State.Sensors.Count > 0);
            bool validVEXState = false;
            int tryCount = 0;

            while (!validVEXState)
            {
                tryCount++;

                // See if the VEX is configured.
                yield return Arbiter.Choice(_vexPort.Get(),
                    delegate(vex.VEXState vState)
                    {
                        vexState = vState;
                    },
                    delegate(Fault fault)
                    {
                        LogError(fault);
                    });

                validVEXState = (vexState != null
                    && vexState.PortSensors != null
                    && vexState.PortSensors.Sensors.Length > 0
                    && vexState.ComPort > 0);

                // If we don't have valid configuration from the VEX
                // wait a little and try again.
                if (!validVEXState)
                {
                    LogVerbose(LogGroups.Console, "Waiting for VEX to be initialized");
                    System.Threading.Thread.Sleep(500);
                }
            }

            configArray.Sensors = new List<VEXBumperConfig>();

            // We do not have a valid configuration from the VEX.
            if (!validVEXState)
            {
                // Do we have a valid prior configuration?
                if (existingBumperConfig)
                {
                    configArray.Sensors = _State.Sensors;
                }
                else
                {
                    // We have no prior configuration
                    VEXBumperConfig bumperConfig = new VEXBumperConfig();
                    bumperConfig.SensorPort = vex.SensorPort.SensorPort_None;
                    bumperConfig.SensorType = vex.SensorDefinition.SensorType.Button;
                    bumperConfig.Name = "Default Bumper 1";
                    bumperConfig.ThresholdLow = 1;
                    bumperConfig.ThresholdHigh = 0xffff;
                    configArray.Sensors.Add(bumperConfig);
                }
            }
            else // We have valid VEX Configuration
            {
                int hardwareIdentifier = 1;
                foreach (vex.PortSensorData sensorConfig in vexState.PortSensors.Sensors)
                {
                    VEXBumperConfig bumperConfig = new VEXBumperConfig();
                    bumperConfig.SensorPort = (vex.SensorPort)hardwareIdentifier;
                    bumperConfig.SensorType = sensorConfig.Type;

                    switch (sensorConfig.Type)
                    {
                        case vex.SensorDefinition.SensorType.None:
                            break;

                        case vex.SensorDefinition.SensorType.Button:
                            {
                                bumperConfig.Name = "Touch Bumper " + hardwareIdentifier.ToString();
                                bumperConfig.ThresholdLow = 1;
                                bumperConfig.ThresholdHigh = 0xffff;
                            }
                            break;

                        case vex.SensorDefinition.SensorType.Limit:
                            {
                                bumperConfig.Name = "Limit Sensor " + hardwareIdentifier.ToString();
                                bumperConfig.ThresholdLow = 1;
                                bumperConfig.ThresholdHigh = 0xffff;
                            }
                            break;

                        case vex.SensorDefinition.SensorType.LightSensor:
                            {
                                bumperConfig.Name = "Analog Input " + hardwareIdentifier.ToString();
                                bumperConfig.ThresholdLow = 512;
                                bumperConfig.ThresholdHigh = 1023;
                            }
                            break;

                        case vex.SensorDefinition.SensorType.Sonar:
                            {
                                bumperConfig.Name = "Sonar Bumper " + hardwareIdentifier.ToString();
                                bumperConfig.ThresholdLow = 0;
                                bumperConfig.ThresholdHigh = 128;
                            }
                            break;

                        case vex.SensorDefinition.SensorType.AnalogInput:
                            {
                                bumperConfig.Name = "Analog Input " + hardwareIdentifier.ToString();
                                bumperConfig.ThresholdLow = 512;
                                bumperConfig.ThresholdHigh = 1023;
                            }
                            break;

                        case vex.SensorDefinition.SensorType.DigitalInput:
                            {
                                bumperConfig.Name = "Digital Input " + hardwareIdentifier.ToString();
                                bumperConfig.ThresholdLow = 1;
                                bumperConfig.ThresholdHigh = 0xffff;
                            }
                            break;

                        case vex.SensorDefinition.SensorType.Potentiometer:
                            {
                                bumperConfig.Name = "Potentiometer " + hardwareIdentifier.ToString();
                                bumperConfig.ThresholdLow = 512;
                                bumperConfig.ThresholdHigh = 1023;
                            }
                            break;

                        case vex.SensorDefinition.SensorType.Gyro:
                            {
                                bumperConfig.Name = "Gyro " + hardwareIdentifier.ToString();
                                bumperConfig.ThresholdLow = 512;
                                bumperConfig.ThresholdHigh = 1023;
                            }
                            break;

                        case vex.SensorDefinition.SensorType.Accelerometer:
                            {
                                bumperConfig.Name = "Accelerometer " + hardwareIdentifier.ToString();
                                bumperConfig.ThresholdLow = 512;
                                bumperConfig.ThresholdHigh = 1023;
                            }
                            break;

                    }

                    #region Merge with prior config
                    // Look to see if this sensor was already configured
                    if (existingBumperConfig)
                    {
                        if (!string.IsNullOrEmpty(bumperConfig.Name))
                        {
                            VEXBumperConfig prevBumperConfig = _State.Sensors.Find(delegate(VEXBumperConfig cfg) { return (int)cfg.SensorPort == hardwareIdentifier; });
                            if (prevBumperConfig != null)
                            {
                                // yes, use the old name.
                                bumperConfig.Name = prevBumperConfig.Name;

                                // if the sensor type has not changed, use the old threshold values.
                                if (prevBumperConfig.SensorType == bumperConfig.SensorType)
                                {
                                    bumperConfig.ThresholdLow = prevBumperConfig.ThresholdLow;
                                    bumperConfig.ThresholdHigh = prevBumperConfig.ThresholdHigh;
                                }
                            }
                        }
                    }
                    #endregion

                    if (!string.IsNullOrEmpty(bumperConfig.Name))
                    {
                        configArray.Sensors.Add(bumperConfig);
                    }
                    hardwareIdentifier++;
                }
            }

            resultPort.Post(configArray);
            yield break;
        }

        /// <summary>
        /// Subscribe to appropriate sensor type and port on VEX
        /// Hook up notifications
        /// and then post success or failure
        /// <param name="successPort"></param>
        /// </summary>
        private IEnumerator<ITask> SubscribeToVEX(Port<bool> successPort)
        {
            //create a custom subscription request
            vex.CustomSubscribeRequestType request = new vex.CustomSubscribeRequestType();

            //select only the sensor and port we want
            //NOTE: this name must match the VEX sensor name.
            request.Sensors = new List<vex.SensorDefinition>();

            foreach (VEXBumperConfig config in _State.Sensors)
            {
                vex.SensorDefinition sensor = new vex.SensorDefinition();
                sensor.Type = config.SensorType;
                sensor.Port = config.SensorPort;
                request.Sensors.Add(sensor);
            }

            //Subscribe to the VEX and wait for a response
            bool subscribed = false;
            yield return
                Arbiter.Choice(_vexPort.SelectiveSubscribe(request, _notificationPort),
                    delegate(SubscribeResponseType Rsp)
                    {
                        //Subscription was successful, start listening for sensor change notifications
                        Activate(
                            Arbiter.Receive<vex.Update>(true, _notificationPort, SensorNotificationHandler)
                        );
                        
                        //update our state with subscription status
                        subscribed = true;
                    },
                    delegate(Fault fault)
                    {
                        LogError("Bumper subscription failed", fault);
                        successPort.Post(false);
                    }
                );

            successPort.Post(subscribed);

            yield break;
        }

        #endregion

        #region Internal Port Handlers

        /// <summary>
        /// Find the hardware configuration for the specified hardware identifier.
        /// </summary>
        /// <param name="hardwareIdentifier"></param>
        /// <returns>The VEXBumperConfig or null</returns>
        private VEXBumperConfig GetBumperConfig(vex.SensorPort sensorPort)
        {
            return _State.Sensors.Find(
                delegate(VEXBumperConfig cfg)
                {
                    return cfg.SensorPort == sensorPort;
                });
        }

        /// <summary>
        /// Handle sensor update message from VEX
        /// </summary>
        private void SensorNotificationHandler(vex.Update notify)
        {
            //update state
            for(int sensorIndex=0; sensorIndex<_State.Sensors.Count; ++sensorIndex)
            {
                VEXBumperConfig sensor = _State.Sensors[sensorIndex];

                bool newContactValue;
                VEXBumperConfig bumperConfig = GetBumperConfig(sensor.SensorPort);
                if (bumperConfig == null)
                {
                    // Old logic works only for touch sensor
                    newContactValue = (notify.Body.PortSensors.Sensors[(int)sensor.SensorPort - 1].Value != 0) ? true : false;
                }
                else
                {
                    switch (notify.Body.PortSensors.Sensors[(int)sensor.SensorPort - 1].Type)
                    {
                        case vex.SensorDefinition.SensorType.Button:
                        case vex.SensorDefinition.SensorType.Limit:
                        case vex.SensorDefinition.SensorType.DigitalInput:
                            ushort value = notify.Body.PortSensors.Sensors[(int)sensor.SensorPort - 1].Value;
                            newContactValue = (value == 0);
                            break;
                        default:
                            newContactValue = ((notify.Body.PortSensors.Sensors[(int)sensor.SensorPort - 1].Value >= bumperConfig.ThresholdLow)
                                            && (notify.Body.PortSensors.Sensors[(int)sensor.SensorPort - 1].Value <= bumperConfig.ThresholdHigh));
                            break;
                    }
                }

                bool changed = (sensor.Pressed != newContactValue);
                sensor.TimeStamp = DateTime.Now;
                sensor.Pressed = newContactValue;

                if (changed)
                {
                    bumper.ContactSensor genericSensor = new bumper.ContactSensor();
                    SendNotification<bumper.Update>(_subMgrPort, _State.ToGenericState(genericSensor, sensorIndex));
                }
            }
        }



        #endregion

        #region Standard Operation Handlers

        /// <summary>
        /// Get the state
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public IEnumerator<ITask> GetHandler(Get get)
        {
            get.ResponsePort.Post(_State);
            yield break;
        }


        /// <summary>
        /// Get the state
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent, PortFieldName = "_genericPort")]
        public IEnumerator<ITask> GetHandler(bumper.Get get)
        {
            bumper.ContactSensorArrayState genericState = new bumper.ContactSensorArrayState();
            get.ResponsePort.Post(_State.ToGenericState(genericState));
            yield break;
        }

        /// <summary>
        /// Get the state to HTTP display
        /// </summary>
        /// <param name="httpGet">Response port to post the state</param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        [ServiceHandler(ServiceHandlerBehavior.Concurrent, PortFieldName = "_genericPort")]
        public IEnumerator<ITask> HttpGetHandler(HttpGet httpGet)
        {
            httpGet.ResponsePort.Post(new HttpResponseType(
                HttpStatusCode.OK,
                _State,
                _transform)
            );
            yield break;
        }

        /// <summary>
        /// Replace the state with the given update
        /// </summary>
        /// <param name="replace">Replacement state</param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        [ServiceHandler(ServiceHandlerBehavior.Exclusive, PortFieldName = "_genericPort")]
        public IEnumerator<ITask> ReplaceHandler(bumper.Replace replace)
        {
            if (replace.Body.Sensors.Count != _State.Sensors.Count)
            {
                Fault fault = Fault.FromException(new InvalidOperationException("Invalid number of sensors"));
                replace.ResponsePort.Post(fault);
                yield break;
            }
            for (int i = 0; i < replace.Body.Sensors.Count; ++i)
            {
                if ((int)_State.Sensors[i].SensorPort != replace.Body.Sensors[i].HardwareIdentifier)
                {
                    Fault fault = Fault.FromException(new InvalidOperationException("Cannot reassign sensor ports"));
                    replace.ResponsePort.Post(fault);
                    yield break;
                }
            }
            _State.FromGenericState(replace.Body);
            SaveState(_State);

            bumper.ContactSensorArrayState genericState = new bumper.ContactSensorArrayState();
            genericState.Sensors = new List<bumper.ContactSensor>();
            SendNotification<bumper.Replace>(_subMgrPort, _State.ToGenericState(genericState));
            replace.ResponsePort.Post(DefaultReplaceResponseType.Instance);

            yield break;
        }

        /// <summary>
        /// Subscribe to updates
        /// </summary>
        /// <param name="subscribe"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        [ServiceHandler(ServiceHandlerBehavior.Exclusive, PortFieldName = "_genericPort")]
        public IEnumerator<ITask> SubscribeHandler(bumper.Subscribe subscribe)
        {
            yield return Arbiter.Choice(
                SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort),
                delegate(SuccessResult success)
                {
                    bumper.ContactSensorArrayState genericState = new bumper.ContactSensorArrayState();
                    genericState.Sensors = new List<bumper.ContactSensor>();
                    SendNotification<bumper.Replace>(_subMgrPort, _State.ToGenericState(genericState));
                },
                delegate(Exception fault)
                {
                    LogError(fault);
                }
            );
        }

        /// <summary>
        /// ReliableSubscribe Handler
        /// </summary>
        /// <param name="subscribe"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        [ServiceHandler(ServiceHandlerBehavior.Exclusive, PortFieldName = "_genericPort")]
        public IEnumerator<ITask> ReliableSubscribeHandler(bumper.ReliableSubscribe subscribe)
        {
            yield return Arbiter.Choice(
                SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort),
                delegate(SuccessResult success)
                {
                    bumper.ContactSensorArrayState genericState = new bumper.ContactSensorArrayState();
                    genericState.Sensors = new List<bumper.ContactSensor>();
                    SendNotification<bumper.Replace>(_subMgrPort, _State.ToGenericState(genericState));
                },
                delegate(Exception fault)
                {
                    LogError(fault);
                }
            );
        }
        
        /// <summary>
        /// Handle the VEX specific bumper configuration
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        [ServiceHandler(ServiceHandlerBehavior.Exclusive, PortFieldName = "_genericPort")]
        public IEnumerator<ITask> UpdateHandler(bumper.Update update)
        {
            foreach(VEXBumperConfig sensor in _State.Sensors)
            {
                if((int)sensor.SensorPort == update.Body.HardwareIdentifier)
                {
                    sensor.Name = update.Body.Name;
                    sensor.Pose = update.Body.Pose;
                    sensor.Pressed = update.Body.Pressed;
                    sensor.TimeStamp = update.Body.TimeStamp;
                }
            }

            update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }
        #endregion
    }
}
